/*=============================================================================
#    The element-comm is a tentative program used to test our ideas.           
#                       Copyright (C)  bgao                                    
#
#     This program is free software; you can redistribute it and/or            
#      modify it under the terms of the GNU General Public License             
#     as published by the Free Software Foundation; either version 2           
#         of the License, or (at your option) any later version.               
#
#     This program is distributed in the hope that it will be useful,          
#    but WITHOUT ANY WARRANTY; without even the implied warranty of           
#     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the            
#             GNU General Public License for more details.                    
#
#    You should have received a copy of the GNU General Public License        
#     along with this program; if not, write to the Free Software             
#      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,             
#                       MA  02110-1301, USA.                                  
#==============================================================================
# $Id$ 
#
# $Author$ 
#
# $Revision$ 
#
# $Date$ 
#
# Description: 
#
=============================================================================*/
#include    "rtos-wsn.h"
#include    "wsn-taskid.h"

static struct wsn_timer * wsn_timer_queue[WSN_MAX_ACTIVE_TIMERS];
static uint8 wsn_active_timers;

//static unsigned long wsn_systime;
unsigned long wsn_systime;

static uint8 timer_task_handle;

#define	timer_expire(t)	    (wsn_timer_queue[t]->expire)
#define time_before(t1, t2)	((int16)((uint16)(t1) - (uint16)(t2)) < 0)
#define time_after(t1, t2)	((int16)((uint16)(t1) - (uint16)(t2)) > 0)

static int8 Wsn_TimerShiftUp(struct wsn_timer * timer, uint8 index)
{
    uint8 parent;
    struct wsn_timer * p_parent;

	if(index >= WSN_MAX_ACTIVE_TIMERS)
		return -EINVAL;

    while ( (parent = index/2) > 0 ) {
	p_parent = wsn_timer_queue[parent];
	if ( !time_after(p_parent->expire, timer->expire) )
	    break;
	/* Swap parent and child */
	wsn_timer_queue[index] = p_parent;
	p_parent->index = index;
	index = parent;
    } 
    wsn_timer_queue[index] = timer;
    timer->index = index;

	return 0;
}

static int8 Wsn_TimerShiftDown(struct wsn_timer * timer, uint8 index)
{
    uint8 child;
    struct wsn_timer * p_child;

	if(index >= WSN_MAX_ACTIVE_TIMERS)
		return -EINVAL;

    /* Shift down this timer */
    while ( (child = index * 2) <= wsn_active_timers ) {
        if ( child < wsn_active_timers ) { /* This node has 2 children */
	    if ( time_before(timer_expire(child+1), timer_expire(child)) )
		child ++;
	}
	p_child = wsn_timer_queue[child];
	if ( !time_before(p_child->expire, timer->expire) )
	    break;
        /* Swap parent and child */
        wsn_timer_queue[index] = p_child;
        p_child->index = index;
        index = child;
    }
    wsn_timer_queue[index] = timer;
    timer->index = index;

	return 0;
}

static int8 Wsn_AddTimer(struct wsn_timer * timer, uint16 expire)
{
	int8 ret;
	timer->expire = expire;
    ret = Wsn_TimerShiftUp(timer, ++wsn_active_timers);

	return ret;
}

static void Wsn_RemoveTimer(struct wsn_timer * timer)
{
    uint8 index;
    
    index = timer->index;
    timer->index = 0;
    /* Move last timer into this place */
    timer = wsn_timer_queue[wsn_active_timers];
    /* If this is last entry, return */
    if ( wsn_active_timers -- == index )
		return;
    if ( index > 1 && time_after(timer_expire(index/2), timer->expire) ) {
        Wsn_TimerShiftUp(timer, index);
    } else {
		Wsn_TimerShiftDown(timer, index);
    }
}

/* Handle timer tick */
void wsn_timer_tick (void)
{
    wsn_systime ++;
    /* If timer task is active, return */
    if ( WSN_TaskID(timer_task_handle) == TIMER_TASKID )
		return;
    if ( wsn_active_timers != 0 && !time_after(timer_expire(1), wsn_systime) ) {
		timer_task_handle = WSN_NewTask (TIMER_TASKID, 0, 0);
		ASSERT(timer_task_handle != 0);
    }
}

void wsn_timer_init (void)
{
    return;
}

uint32 WSN_GetSystemTime(void)
{
    return wsn_systime;
}

uint16 WSN_GetSystemTime_16(void)
{
    return (uint16)wsn_systime;
}

void WSN_InitTimer(struct wsn_timer * timer, timer_func func)
{
    timer->func = func;
    timer->index = 0;
}
/*timeout can't larger than 32767*/
int8 WSN_StartTimer(struct wsn_timer * timer, uint16 timeout)
{
    uint8 sched_flag = wsn_disable_sched;
	int8 ret;
    if ( sched_flag == SCHED_UNLOCK )
        WSN_SchedOff();
    if ( WSN_IsTimerOn(timer) ) {
		ASSERT (wsn_timer_queue[timer->index] == timer);  
		Wsn_RemoveTimer(timer);
    }
    ret = Wsn_AddTimer(timer, (uint16)wsn_systime + timeout);
    if ( sched_flag == SCHED_UNLOCK )
        WSN_SchedOn();
	return ret;
}

void WSN_StopTimer(struct wsn_timer * timer)
{
    uint8 sched_flag = wsn_disable_sched;

    if ( sched_flag == SCHED_UNLOCK)
		WSN_SchedOff();
    if ( WSN_IsTimerOn(timer) ) {
		ASSERT (wsn_timer_queue[timer->index] == timer); 
		Wsn_RemoveTimer(timer);
    }
    if ( sched_flag == SCHED_UNLOCK )
        WSN_SchedOn();
}

int Wsn_TimerTask(int event)
{
    struct wsn_timer *timer;

    WSN_SchedOff();
    while ( wsn_active_timers > 0 ) {
		timer = wsn_timer_queue[1];
		if ( time_after(timer->expire, wsn_systime) )
	    	break;
		/* This timer is expired, take it out from head */
		Wsn_RemoveTimer(timer);
        WSN_SchedOn();
		timer->func(timer);
        WSN_SchedOff();
    }
    WSN_SchedOn();
    return 1;
}

